/*============================= Joint PD ==============================*/
/**
 * FSM State that allows PD control of the joints.
 */

#include "FSM_State_JointPD.h"

/**
 * Constructor for the FSM State that passes in state specific info to
 * the generic FSM State constructor.
 *
 * @param _controlFSMData holds all of the relevant control data
 */
template <typename T>
FSM_State_JointPD<T>::FSM_State_JointPD(ControlFSMData<T>* _controlFSMData)
    : FSM_State<T>(_controlFSMData, FSM_StateName::JOINT_PD, "JOINT_PD"), _ini_jpos(dog::num_act_joint) {
    rcCommand = this->_data->_desiredStateCommand->rcCommand;
    _legController = this->_data->_legController;
}

template <typename T>
void FSM_State_JointPD<T>::onEnter() {
    // Default is to not transition
    this->nextStateName = this->stateName;

    // Reset the transition data
    this->transitionData.zero();

    // Reset counter
    iter = 0;

    for (size_t leg(0); leg < 4; ++leg) {
        for (size_t jidx(0); jidx < 3; ++jidx) {
            _ini_jpos[3 * leg + jidx] = _legController->datas[leg].q[jidx];
        }
        _legController->commands[leg].qDes = _legController->datas[leg].q;
    }
    _cur_jpos = _ini_jpos;

    // test
    this->_data->_gaitScheduler->gaitData._nextGait = GaitType::TROT;
}

/**
 * Calls the functions to be executed on each control loop iteration.
 */
template <typename T>
void FSM_State_JointPD<T>::run() {
    Vec3<double> Kp, Kd;
    Vec3<T> qDes, qdDes, pDes, vDes; 
    uint8_t cmd;
    qdDes << 0, 0, 0;

    // control robot state according to rc command
    cmd = rcCommand->test_type[1];
    for (size_t leg(0); leg < 4; ++leg) {
        for (size_t jidx(0); jidx < 3; ++jidx) {
            _legController->commands[leg].qDes[jidx] = _cur_jpos[3 * leg + jidx];
        }
    }
    switch (rcCommand->test_type[0]) {
        case QT_Joint:
            _cur_jpos[cmd] = rcCommand->test_data[0];
            qDes << _cur_jpos[cmd / 3 * 3], _cur_jpos[cmd / 3 * 3 + 1], _cur_jpos[cmd / 3 * 3 + 2];
            this->jointPDControl(cmd / 3, qDes, qdDes);
            break;

        case QT_Foot:
            pDes << rcCommand->test_data[0], rcCommand->test_data[1], rcCommand->test_data[2];
            Kp << rcCommand->test_data[3], rcCommand->test_data[4], rcCommand->test_data[5];
            vDes << rcCommand->test_data[6], rcCommand->test_data[7], rcCommand->test_data[8];
            Kd << rcCommand->test_data[9], rcCommand->test_data[10], rcCommand->test_data[11];
            // this->cartesianImpedanceControl(cmd, pDes, vDes, Kp, Kd);
            this->BezierControl(Kp, Kd);
            break;
        default:
            break;
    }
}

template <typename T>
void FSM_State_JointPD<T>::BezierControl(Vec3<double> kp_cartesian, Vec3<double> kd_cartesian){
    //轨迹点
    float stepLength = 0.12;
    float Height = 0.02;
    Vec3<float> initialPosition, finalPosition;
    Mat3<float> kpMat, kdMat;
    initialPosition << -stepLength / 2.0, 0, -0.28;
    finalPosition << stepLength / 2.0, 0, -0.28;
    kpMat << kp_cartesian[0], 0, 0, 0,
    kp_cartesian[1], 0, 0, 0,
    kp_cartesian[2];
    kdMat << kd_cartesian[0], 0, 0, 0,
    kd_cartesian[1], 0, 0, 0,
    kd_cartesian[2];
    
    for (int leg = 0; leg < 4; leg++) {
        //步态信息
        Vec3<float> pDes, vDes, qDes, qTemp;
        // printf("contact:%d\n", this->_data->_gaitScheduler->gaitData.contactStateScheduled(leg));
        //摆动相
        if (!this->_data->_gaitScheduler->gaitData.contactStateScheduled(leg)) {
            _footSwingTrajectory.setHeight(Height);
            _footSwingTrajectory.setInitialPosition(initialPosition);
            _footSwingTrajectory.setFinalPosition(finalPosition);
            _footSwingTrajectory.computeSwingTrajectoryBezier(
                 this->_data->_gaitScheduler->gaitData.phaseSwing(leg), 0.25);
            pDes = _footSwingTrajectory.getPosition();
            vDes = _footSwingTrajectory.getVelocity();
            this->_data->_legController->commands[leg].pDes= pDes;
            this->_data->_legController->commands[leg].vDes= vDes;
            this->_data->_legController->commands[leg].kpCartesian = kpMat;
            this->_data->_legController->commands[leg].kdCartesian = kdMat;
            this->_data->_legController->setControlMode(Abad_Pos);
            if(leg == 0) this->_data->_legController->setEnabled(leg, true);

            printf("pDes:%f, %f, %f\n", pDes[0],  pDes[1],  pDes[2]);
            printf("vDes:%f, %f, %f\n", vDes[0],  vDes[1],  vDes[2]);
            printf("Kp:%f, %f, %f\n", kp_cartesian[0],  kp_cartesian[1],  kp_cartesian[2]);
            printf("Kd:%f, %f, %f\n", kd_cartesian[0],  kd_cartesian[1],  kd_cartesian[2]);
        }
        else{
            pDes = initialPosition;
            computeAuxiliaryAngle(*(this->_data->_quadruped), pDes, &qTemp);
            qDes <<  qTemp[0], qTemp[1] + qTemp[2], qTemp[2] - qTemp[1];
            this->_data->_legController->commands[leg].qDes= qDes;
            this->_data->_legController->setControlMode(All_Pos);
            if(leg == 0) this->_data->_legController->setEnabled(leg, true);
        }
    }
}

/**
 * Manages which states can be transitioned into either by the user
 * commands or state event triggers.
 *
 * @return the enumerated FSM state name to transition into
 */
template <typename T>
FSM_StateName FSM_State_JointPD<T>::checkTransition() {
    this->nextStateName = this->stateName;

    iter++;

    // Switch FSM control mode
    switch ((int)this->_data->controlParameters->control_mode) {
        case K_JOINT_PD:
            // Normal operation for state based transitions
            break;

        case K_IMPEDANCE_CONTROL:
            // Requested change to impedance control
            this->nextStateName = FSM_StateName::IMPEDANCE_CONTROL;

            // Transition time is 1 second
            this->transitionDuration = 1.0;
            break;

        case K_STAND_UP:
            // Requested change to impedance control
            this->nextStateName = FSM_StateName::STAND_UP;

            // Transition time is immediate
            this->transitionDuration = 0.0;
            break;

        case K_BALANCE_STAND:
            // Requested change to balance stand
            this->nextStateName = FSM_StateName::BALANCE_STAND;
            break;

        case K_PASSIVE:
            // Requested change to BALANCE_STAND
            this->nextStateName = FSM_StateName::PASSIVE;

            // Transition time is immediate
            this->transitionDuration = 0.0;

            break;

        default:
            std::cout << "[CONTROL FSM] Bad Request: Cannot transition from " << K_JOINT_PD << " to "
                      << this->_data->controlParameters->control_mode << std::endl;
    }

    // Get the next state
    return this->nextStateName;
}

/**
 * Handles the actual transition for the robot between states.
 * Returns true when the transition is completed.
 *
 * @return true if transition is complete
 */
template <typename T>
TransitionData<T> FSM_State_JointPD<T>::transition() {
    // Switch FSM control mode
    switch (this->nextStateName) {
        case FSM_StateName::IMPEDANCE_CONTROL:

            iter++;
            if (iter >= this->transitionDuration * 1000) {
                this->transitionData.done = true;
            } else {
                this->transitionData.done = false;
            }
            break;

        case FSM_StateName::STAND_UP:
            this->transitionData.done = true;

            break;

        case FSM_StateName::BALANCE_STAND:
            this->transitionData.done = true;

            break;

        case FSM_StateName::PASSIVE:
            this->turnOffAllSafetyChecks();

            this->transitionData.done = true;

            break;

        default:
            std::cout << "[CONTROL FSM] Bad Request: Cannot transition from " << K_JOINT_PD << " to "
                      << this->_data->controlParameters->control_mode << std::endl;
    }
    // Finish transition
    this->transitionData.done = true;

    // Return the transition data to the FSM
    return this->transitionData;
}

/**
 * Cleans up the state information on exiting the state.
 */
template <typename T>
void FSM_State_JointPD<T>::onExit() {
    // Nothing to clean up when exiting
}

// template class FSM_State_JointPD<double>;
template class FSM_State_JointPD<float>;
